package Behaviors;
import Behaviors.NxBehavior;
import Utils.*;

import javaclient3.*;
/**
 * 
 *************************************
 ***********************************
 * @author jonathan
 */
public class NxMSGoToPointNew extends NxBehavior{
    
    private double maxDist = 4.0f;
    private double minDist = 0.1f;
    
    private float exp_param = 3.0f;
    
    public static final int MODE_LINEAR = 0;
    public static final int MODE_EXPONENTIAL = 1;
    
    private int atractorMode;
    
    
    
    //float maxVel= 0.8f;
    private boolean perceptionSchema=false;
    
    public Position2DInterface pos;
    NxBehavior embedded;
    public Vector2d goalPoint;

    public NxMSGoToPointNew(String name,Position2DInterface pos, Vector2d goalPoint, double maxDist, double minDist, int atractorMode) {
        super(name);
        this.pos = pos;
        this.goalPoint = goalPoint;
        this.atractorMode = atractorMode;
        this.setParamsDist(minDist, maxDist);
    }
    
    public NxMSGoToPointNew(String name,NxBehavior position,Position2DInterface pos,  double maxDist, double minDist, int atractorMode) {
        super(name);
        this.pos=pos;
        embedded=(NxBehavior)position;
        perceptionSchema=true;
        this.atractorMode = atractorMode;
        this.setParamsDist(minDist, maxDist);
    }
    
    public Vector2d Value() {
        
        if(perceptionSchema)
        {
            goalPoint = (Vector2d)embedded.Value();
        }

        //while(!pos.isDataReady());
        //get position of robot
        //Pose2d pose = pos.read(RealPosition2d.READ_MODE_NO_WAIT);

        Vector2d myPos = Position2DHelper.getPosition(pos);;

        //print position of robot
        //System.out.println("MSGOTOPOINT: Robot position vector:");
        
        //myPos.print(myPos.MODE_XY);
        
        //Vector in direction to point
        Vector2d atractor=goalPoint.sub(myPos);         
        
        //print attractor vector
        //System.out.println("MSGOTOPOINT: Attractor vector:");
        //atractor.print(atractor.MODE_MA);
        
        //Now define the magnitude based on the chosen mode 
        switch(atractorMode){
            
            case MODE_LINEAR:
                atractor = convertLinearMode(atractor);
                break;
            case MODE_EXPONENTIAL:
                atractor = convertExponentialMode(atractor);
                break;
        
        
        }
        //if(atractor.getMag()<10.0f)
            //atractor.setMag(0.0f);
        if(enabled)
            return atractor;
        else
            return new Vector2d();

    }

    //define la variable maxDist
    public void setMaxDist(float maxDist)
    {
        this.maxDist = maxDist;
    }
    //define la variable minDist
    public void setMinDist(float minDist)
    {
        this.minDist = minDist;
    }
    //define las dos variables anteriores
    public void setParamsDist(double minDist, double maxDist)
    {
        this.maxDist = maxDist;
        this.minDist = minDist;
    }



    private Vector2d convertExponentialMode(Vector2d atractor){
        
        float x = (float)atractor.getMag();
        
        //if object is in the closest area, then shutdown atractor
        if(x<=minDist)
            return new Vector2d(0,0,Vector2d.MODE_XY);
        else{
            float newMag =(float)( 1 - Math.exp(-exp_param*x));

            atractor.setMag(newMag);
        }
        //atractor.scale(maxVel);
        
        return atractor;
        
        
        
    
    }
    
    
    private Vector2d convertLinearMode(Vector2d atractor){
        
        //If vector greater than influence area, then normalize, else scale and 
        //minimize according to distance to goalPoint
        
            if(atractor.getMag()>maxDist){
                atractor=atractor.normalize();
                //return atractor.scale(maxVel);
                return atractor;
                
            }
            else{
                if(atractor.getMag()<=minDist){
                    
                                 
                    return new Vector2d(0,0,Vector2d.MODE_XY);
                }                    
                else
                    //return atractor.scale(1/maxDist*maxVel);
                    return atractor.scale(1/maxDist);
            }
        
    
    }
    //distancia a la cual el vector de velocidad debe disminuir al 37% (0.37).
    public void setExponentialDistance(float x){
        
        exp_param = (float)-Math.log(1 - Math.sqrt(2)/2.0f) / x;
        
        System.out.println(exp_param);
    
    }

    public void changeGoalPoint(Vector2d goalPoint)
    {
        this.goalPoint = goalPoint;
    }


}
